{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial 9: Custom Controllers\n",
    "\n",
    "This tutorial walks through the process of defining controllers for the lateral and longitudinal movement of human-driven vehicles within a network. Such controllers may be necessary in order to model types of human behavior not already supported in SUMO. Controllers can be defined by adding to the existing controllers defined in the directory `flow/controllers/`. \n",
    "\n",
    "Here, we will discuss Flow's `BaseController` class and then build two controllers: a longitudinal Intelligent Driver Model controller [CITE] and a lateral controller that attempts to move all vehicles into the same lane.\n",
    "\n",
    "When adding a custom controller, ensure changes are reflected in `flow/controllers/__init__.py` under the import statements as well as in the list `__all__`. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## 1 Longitudinal Controller\n",
    "\n",
    "### 1.1 BaseController\n",
    "\n",
    "Flow's `BaseController` class is an abstract class to use when implementing longitudinal controllers. It includes failsafe methods and the `get_action` method called by Flow's `core.base` module. `get_action` adds noise to actions and runs failsafes, if specified. `BaseController` does not implement `get_accel`; that method should be implemented in any controllers that are subclasses of `BaseController`. \n",
    "\n",
    "As such, any longitudinal controller must import `BaseController`. We also import NumPy in order to use some mathematical functions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "from flow.controllers.base_controller import BaseController"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.2 Controller Initialization\n",
    "\n",
    "Here we initialize an IDM controller class and the `__init__` function storing class attributes.\n",
    "\n",
    "The Intelligent Driver Model is a car-following model specifying vehicle dynamics by a differential equation for acceleration $\\dot{v}$. The differential equation follows:\n",
    "\n",
    "$$\\dot{v} = a \\left[ 1- \\left( \\frac{v}{v_0} \\right)^\\delta -\\left( \\frac{s^*}{h} \\right)^2 \\right] \\textbf{, with } \\ s^* := s_0 + \\max \\left( 0, vT + \\frac{v\\Delta v}{2\\sqrt{ab}} \\right)$$\n",
    "\n",
    "The IDM parameters are: desired speed $v_0$, time gap $T$, min gap $s_0$, acceleration exponent $\\delta$, acceleration term $a$, and comfortable deceleration $b$. $h$ is the vehicle headway (the distance to the vehicle ahead) and $\\Delta v$ is the velocity difference compared to the lead vehicle (current velocity - lead velocity)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class IDMController(BaseController):\n",
    "    def __init__(self, veh_id, v0=30, T=1, a=1, b=1.5, \n",
    "                 delta=4, s0=2, s1=0, time_delay=0.0, \n",
    "                 dt=0.1, noise=0, fail_safe=None, car_following_params=None):\n",
    "        \"\"\"\n",
    "        veh_id: str\n",
    "            unique vehicle identifier\n",
    "        car_following_params: SumoCarFollowingParams\n",
    "            see parent class\n",
    "        v0: float, optional\n",
    "            desirable velocity, in m/s (default: 30)\n",
    "        T: float, optional\n",
    "            safe time headway, in s (default: 1)\n",
    "        b: float, optional\n",
    "            comfortable deceleration, in m/s2 (default: 1.5)\n",
    "        delta: float, optional\n",
    "            acceleration exponent (default: 4)\n",
    "        s0: float, optional\n",
    "            linear jam distance, in m (default: 2)\n",
    "        s1: float, optional\n",
    "            nonlinear jam distance, in m (default: 0)\n",
    "        dt: float, optional\n",
    "            timestep, in s (default: 0.1)\n",
    "        noise: float, optional\n",
    "            std dev of normal perturbation to the acceleration (default: 0)\n",
    "        fail_safe: str, optional\n",
    "            type of flow-imposed failsafe the vehicle should posses, defaults\n",
    "            to no failsafe (None)\n",
    "        \"\"\"\n",
    "        \n",
    "        BaseController.__init__(self, veh_id, car_following_params,\n",
    "                                delay=time_delay, fail_safe=fail_safe,\n",
    "                                noise=noise)\n",
    "        self.v0 = v0\n",
    "        self.T = T\n",
    "        self.a = a\n",
    "        self.b = b\n",
    "        self.delta = delta\n",
    "        self.s0 = s0\n",
    "        self.s1 = s1\n",
    "        self.dt = dt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.3 Acceleration Command\n",
    "\n",
    "Next, we implement the acceleration equation specified by IDM: \n",
    "\n",
    "$$\\dot{v} = a \\left[ 1- \\left( \\frac{v}{v_0} \\right)^\\delta -\\left( \\frac{s^*}{h} \\right)^2 \\right] \\textbf{, with } \\ s^* := s_0 + \\max \\left( 0, vT + \\frac{v\\Delta v}{2\\sqrt{ab}} \\right)$$\n",
    "\n",
    "The vehicle's velocity `v` is fetched by getter method `get_speed` of the environment's vehicles object, as is the id of the lead vehicle `lead_id` and headway `h`. \n",
    "\n",
    "Input-checking to ensure that overly small headways are not used is performed, as well as a step to set $s^*$ properly when no car is ahead of the vehicle being controlled. If there is a lead vehicle, $s^*$ is calculated as described, and then the IDM acceleration is returned."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class IDMController(BaseController):\n",
    "    def __init__(self, veh_id, v0=30, T=1, a=1, b=1.5, \n",
    "                 delta=4, s0=2, s1=0, time_delay=0.0, \n",
    "                 dt=0.1, noise=0, fail_safe=None, car_following_params=None):\n",
    "        \"\"\"Docstring eliminated here for brevity\"\"\"\n",
    "        BaseController.__init__(self, veh_id, car_following_params,\n",
    "                                delay=time_delay, fail_safe=fail_safe,\n",
    "                                noise=noise)\n",
    "        self.v0 = v0\n",
    "        self.T = T\n",
    "        self.a = a\n",
    "        self.b = b\n",
    "        self.delta = delta\n",
    "        self.s0 = s0\n",
    "        self.s1 = s1\n",
    "        self.dt = dt\n",
    "\n",
    "        \n",
    "    ##### Below this is new code #####\n",
    "    def get_accel(self, env):\n",
    "        v = env.k.vehicle.get_speed(self.veh_id)\n",
    "        lead_id = env.k.vehicle.get_leader(self.veh_id)\n",
    "        h = env.k.vehicle.get_headway(self.veh_id)\n",
    "\n",
    "        # negative headways may be registered by sumo at intersections/\n",
    "        # junctions. Setting them to 0 causes vehicles to not move; therefore,\n",
    "        # we maintain these negative headways to let sumo control the dynamics\n",
    "        # as it sees fit at these points.\n",
    "        if abs(h) < 1e-3:\n",
    "            h = 1e-3\n",
    "\n",
    "        if lead_id is None or lead_id == '':  # no car ahead\n",
    "            s_star = 0\n",
    "        else:\n",
    "            lead_vel = env.k.vehicle.get_speed(lead_id)\n",
    "            s_star = self.s0 + max(\n",
    "                0,\n",
    "                v * self.T + v*(v-lead_vel) / (2*np.sqrt(self.a*self.b)))\n",
    "\n",
    "        return self.a * (1 - (v/self.v0)**self.delta - (s_star/h)**2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2 Lateral Controller\n",
    "\n",
    "### 2.1 BaseLaneChangeController\n",
    "\n",
    "In this section we will implement a lane-change controller that sends lane-change commands to move a vehicle into lane 2. Flow includes a BaseLaneChangeController abstract class that functions similarly to the BaseController class, implementing safety-checking utility methods for control.\n",
    "\n",
    "First, we import the BaseLaneChangeController object and define a lane-change controller class, but leave method definition until the next step. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from flow.controllers.base_lane_changing_controller import BaseLaneChangeController\n",
    "\n",
    "class LaneZeroController(BaseLaneChangeController):\n",
    "    \"\"\"A lane-changing model used to move vehicles into lane 0.\"\"\"\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.2 Lane-Change Command\n",
    "\n",
    "Lane-change controllers must implement the method `get_lane_change_action`. Actions in Flow are specified as directions, which are a number out of `[-1, 0, 1]`. Lane 0 is the farthest-right, so the direction -1 is a lane change to the right. \n",
    "\n",
    "This `get_lane_change_action` implementation fetches the current lane the vehicle is in, using the `get_lane` method of the Vehicles object and passing in `self.veh_id`. If the vehicle is in a lane different from lane 0, it must have a lane number above 0, since lane numbers are positive in SUMO. In that case, a lane-change to the right is specified by returning the direction -1. If the vehicle is in lane 0, then the direction 0 is returned."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class LaneZeroController(BaseLaneChangeController):\n",
    "    \"\"\"A lane-changing model used to move vehicles into lane 0.\"\"\"\n",
    "\n",
    "    ##### Below this is new code #####\n",
    "    def get_lane_change_action(self, env):\n",
    "        current_lane = env.k.vehicle.get_lane(self.veh_id)\n",
    "        if current_lane > 0:\n",
    "            return -1\n",
    "        else:\n",
    "            return 0"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
